classdef KalmFilt < matlab.System & matlab.system.mixin.Propagates ...
         & matlab.system.mixin.Nondirect
    % KalmFilt Kalman滤波算法解算周期
    %
    % NOTE: When renaming the class name KalmFilt, the file name
    % and constructor name must be updated to use the class name.
    %
    % This template includes most, but not all, possible properties, attributes,
    % and methods that you can implement for a System object in Simulink.
    
    % Public, tunable properties
    properties
        
    end
    
    % Public, non-tunable properties
    properties(Nontunable)
        Cfg_nPhilambdalambda (1, 1) double {mustBeInteger} = 2
        Cfg_nPhilambday (1, 1) double {mustBeInteger} = 2
        Cfg_ExtrapAlgo (1, 1) KalmFiltExtrapAlgo = KalmFiltExtrapAlgo.ITER
        Cfg_UpdAlgo (1, 1) KalmFiltUpdAlgo = KalmFiltUpdAlgo.JOSEPH
        Cfg_EnableNumCondCtrl (1, 1) logical = false
        Cfg_MaxObsLen (1, 1) double {mustBeInteger} = 10
        
        Par_DtExtrap (1, 1) double {mustBePositive} = 0.005
        Par_PDiagMin (:, 1) double {mustBeNonnegative}
        Par_PDiagMax (:, 1) double {mustBeNonnegative}
        
        InitCon_x (:, 1) double {mustBeReal}
        InitCon_P (:, :) double {mustBeNonnegative}
        InitCon_F_1 (:, :) double {mustBeReal}
        InitCon_GPQGPT_1 (:, :) double {mustBeReal}
        
        Cfg_FcnMTimes_Extrap (1, 1) = @kfextrapdefaultmtimes;
        Cfg_FcnMTimes_Upd (1, 1) = @kfupddefaultmtimes;
    end
    
    properties(DiscreteState)
        DS_ExtrapCycCnt
        DS_UpdCnt
        DS_x
        DS_P
        DS_F_1
        DS_GPQGPT_1
        DS_MeasResidChkPass
        DS_chiSqChkPass
        DS_MeasResid
        DS_MeasResidCovMatDiagSqrt
        DS_chiSqIndex
        DS_K
        DS_Phi
        DS_GammaPQGammaPT
    end
    
    % Pre-computed constants
    properties(Access = private)
        
    end
    
    methods
        % Constructor
        function obj = KalmFilt(varargin)
            % Support name-value pair arguments when constructing object
            setProperties(obj,nargin,varargin{:})
        end
        
        function [out, auxOut] = setState(obj, u, P0)
             % 状态向量重设
            obj.DS_x = obj.DS_x + u;
            if ~any(any(isnan(in.P0)))
                obj.DS_P = P0;
            end
            [out, auxOut] = obj.output();
        end
        
        function [out, auxOut] = predict(obj, F, GPQGPT)
            % 状态估计及误差协方差外推解算周期
            obj.update(obj, F, GPQGPT);
            [out, auxOut] = obj.output();
        end
        
        function [out, auxOut] = correct(obj, zObs, H, GammaMRGammaMT, enableMeasResidChk, chiSqChkThreshold)
            % 状态估计及误差协方差更新解算周期
            % 量测残差检验与χ^2检
            [obj.DS_MeasResid, obj.DS_chiSqIndex, ...
                obj.DS_MeasResidChkPass, obj.DS_chiSqChkPass, obj.DS_MeasResidCovMatDiagSqrt] ...
                = chkmeaserr(obj.DS_x, obj.DS_P, zObs, H, GammaMRGammaMT, ...
                enableMeasResidChk, chiSqChkThreshold);
            if obj.DS_chiSqChkPass && all(obj.DS_MeasResidChkPass)
                % 检验通过进行更新
                switch obj.Cfg_UpdAlgo
                    case int8(KalmFiltUpdAlgo.JOSEPH)
                        [obj.DS_x, obj.DS_P, obj.DS_K] = kalmfiltupd_joseph(obj.DS_x, obj.DS_P, zObs, H, GammaMRGammaMT, obj.Cfg_FcnMTimes_Upd);
                    case int8(KalmFiltUpdAlgo.JOSEPH_SIMPLE)
                        [obj.DS_x, obj.DS_P, obj.DS_K] = kalmfiltupd_joseph_simple(obj.DS_P, zObs, H, GammaMRGammaMT, obj.Cfg_FcnMTimes_Upd);
                    otherwise
                        [obj.DS_x, obj.DS_P, obj.DS_K] = kalmfiltupd(obj.DS_x, obj.DS_P, zObs, H, GammaMRGammaMT);
                end
                obj.DS_UpdCnt = obj.DS_UpdCnt + 1;
                if  obj.Cfg_EnableNumCondCtrl
                    obj.DS_P = covmatnumcondctrl(obj.DS_P, obj.Par_PDiagMin, obj.Par_PDiagMax);
                end
            else
                warning('KalmFilt:chkFail', ['Measurement residule check and/or chi-square check failed, the observation after the ' int2str(obj.DS_UpdCnt) '-th update is ignored.']);
            end
            % 输出
            [out, auxOut] = obj.output();
        end
    end
    
    methods(Access = protected)
        %% Common functions
        function setupImpl(obj)
            % Perform one-time calculations, such as computing constants
        end
        
        function resetImpl(obj)
            % Initialize / reset discrete-state properties
            obj.DS_ExtrapCycCnt = 0;
            obj.DS_UpdCnt = 0;
            obj.DS_x = obj.InitCon_x;
            obj.DS_P = obj.InitCon_P;
            obj.DS_F_1 = obj.InitCon_F_1;
            obj.DS_GPQGPT_1 = obj.InitCon_GPQGPT_1;
            obj.DS_MeasResidChkPass = true(obj.Cfg_MaxObsLen, 1);
            obj.DS_chiSqChkPass = true;
            obj.DS_MeasResid = NaN(obj.Cfg_MaxObsLen, 1);
            obj.DS_MeasResidCovMatDiagSqrt = NaN(obj.Cfg_MaxObsLen, 1);
            obj.DS_chiSqIndex = NaN;
            obj.DS_K = NaN(size(obj.InitCon_x, 1), obj.Cfg_MaxObsLen);
            obj.DS_Phi = NaN(size(obj.InitCon_P));
            obj.DS_GammaPQGammaPT = NaN(size(obj.InitCon_P));
        end
        
        function validatePropertiesImpl(obj)
            % Validate related or interdependent property values
            if (size(obj.InitCon_P, 1)~=length(obj.InitCon_x)) ...
                    || (size(obj.InitCon_P, 2)~=length(obj.InitCon_x))
                warning('KalmFilt:stateVecLen', 'The length of initial state vector is not the same with the size of error covariance matrix.');
            end
        end
        
        %% Simulink functions
        function ds = getDiscreteStateImpl(obj)
            % Return structure of properties with DiscreteState attribute
            ds = struct([]);
        end
        
        function flag = isInputSizeMutableImpl(obj, index)
            % Return false if input size cannot change
            % between calls to the System object
            flag = false;
        end
        
        function [out, out2] = getOutputSizeImpl(obj)
            % Return size for each output port
            out = [1 1];
            out2 = [1 1];
            
            % Example: inherit size from first input port
            % out = propagatedInputSize(obj,1);
        end

        function [out, out2] = getOutputDataTypeImpl(obj)
            % Return data type for each output port
            out = 'struct';
            out2 = 'struct';
            
            % Example: inherit data type from first input port
            % out = propagatedInputDataType(obj,1);
        end

        function [out,out2] = isOutputComplexImpl(obj)
            % Return true for each output port with complex data
            out = false;
            out2 = false;

            % Example: inherit complexity from first input port
            % out = propagatedInputComplexity(obj,1);
        end

        function [out,out2] = isOutputFixedSizeImpl(obj)
            % Return true for each output port with fixed size
            out = true;
            out2 = true;

            % Example: inherit fixed-size status from first input port
            % out = propagatedInputFixedSize(obj,1);
        end
        
        function updateImpl(obj, F, GPQGPT)
            % Update discrete states as a function of input u
            % 状态估计及误差协方差外推解算周期
            switch obj.Cfg_ExtrapAlgo
                case int8(KalmFiltExtrapAlgo.ITER)
                    [obj.DS_x, obj.DS_P, obj.DS_Phi, obj.DS_GammaPQGammaPT] = kalmfiltextrap_iter(obj.DS_x, obj.DS_P, ...
                        (F+obj.DS_F_1)/2*obj.Par_DtExtrap, (GPQGPT+obj.DS_GPQGPT_1)/2*obj.Par_DtExtrap, ...
                        obj.Cfg_nPhilambdalambda, obj.Cfg_nPhilambday);
                otherwise
                    [obj.DS_x, obj.DS_P, obj.DS_Phi, obj.DS_GammaPQGammaPT] = kalmfiltextrap(obj.DS_x, obj.DS_P, ...
                        (F+obj.DS_F_1)/2, (GPQGPT+obj.DS_GPQGPT_1)/2, obj.Par_DtExtrap, obj.Cfg_nPhilambdalambda, obj.Cfg_FcnMTimes_Extrap);
            end
            obj.DS_ExtrapCycCnt = obj.DS_ExtrapCycCnt + 1;
            obj.DS_F_1 = F;
            obj.DS_GPQGPT_1 = GPQGPT;
        end
        
        function [out, auxOut] = outputImpl(obj, F, GPQGPT)
            % Calculate output y as a function of discrete states and
            % direct feedthrough inputs
            out.extrapCycCnt = obj.DS_ExtrapCycCnt;
            out.updCnt = obj.DS_UpdCnt;
            out.x = obj.DS_x;
            out.P = obj.DS_P;
            out.measResidChkPass = obj.DS_MeasResidChkPass;
            out.chiSqChkPass = obj.DS_chiSqChkPass;
            
            auxOut.measResid = obj.DS_MeasResid;
            auxOut.measResidCovMatDiagSqrt = obj.DS_MeasResidCovMatDiagSqrt ;
            auxOut.chiSqIndex = obj.DS_chiSqIndex ;
            auxOut.K = obj.DS_K;
            auxOut.Phi = obj.DS_Phi;
            auxOut.GammaPQGammaPT = obj.DS_GammaPQGammaPT ;
        end

        function [flag, flag2] = isInputDirectFeedthroughImpl(obj, F, GPQGPT)
            % Return true if input u is needed to calculate the output at
            % the same time
            flag = false;
            flag2 = false;
        end
    end
end